{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# Karcher Mean Factors"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n",
        "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n",
        "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n",
        "\n",
        "Functions/Classes:\n",
        "*   `FindKarcherMean`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n",
        "*   `KarcherMeanFactor<T>`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/KarcherMeanFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n",
        "from gtsam.symbol_shorthand import R"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "find_header_md"
      },
      "source": [
        "## 1. `FindKarcherMean`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "find_desc_md"
      },
      "source": [
        "Computes the Karcher mean of a list of rotations."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "find_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Input Rotations (Yaw angles):\n",
            "  0.100\n",
            "  0.150\n",
            "  0.050\n",
            "  0.120\n",
            "\n",
            "Computed Karcher Mean (Yaw angle): 0.105\n"
          ]
        }
      ],
      "source": [
        "# Create a list of Rot3 objects\n",
        "rotations = gtsam.Rot3Vector()\n",
        "rotations.append(Rot3.Yaw(0.1))\n",
        "rotations.append(Rot3.Yaw(0.15))\n",
        "rotations.append(Rot3.Yaw(0.05))\n",
        "rotations.append(Rot3.Yaw(0.12))\n",
        "\n",
        "# Compute the Karcher mean\n",
        "karcher_mean = FindKarcherMeanRot3(rotations)\n",
        "\n",
        "print(\"Input Rotations (Yaw angles):\")\n",
        "for r in rotations: print(f\"  {r.yaw():.3f}\")\n",
        "\n",
        "print(f\"\\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}\")\n",
        "# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_header_md"
      },
      "source": [
        "## 2. `KarcherMeanFactor<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_desc_md"
      },
      "source": [
        "Creates a factor that constrains the rotational average of a set of `Rot3` variables.\n",
        "It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is $\\sqrt{\\beta} I_{3x3}$, and the error vector is zero. The `beta` parameter (optional, defaults to 1) controls the strength (precision) of the constraint."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "factor_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "KarcherMeanFactorRot3:   keys = { r0 r1 r2 }\n",
            "sqrt(beta): 10.0\n",
            "\n",
            "Jacobian for R(0):\n",
            "[[10.  0.  0.]\n",
            " [ 0. 10.  0.]\n",
            " [ 0.  0. 10.]]\n",
            "Jacobian for R(1):\n",
            "[[10.  0.  0.]\n",
            " [ 0. 10.  0.]\n",
            " [ 0.  0. 10.]]\n",
            "Jacobian for R(2):\n",
            "[[10.  0.  0.]\n",
            " [ 0. 10.  0.]\n",
            " [ 0.  0. 10.]]\n",
            "Error vector b:\n",
            "[0. 0. 0.]\n"
          ]
        }
      ],
      "source": [
        "keys = [R(0), R(1), R(2)]\n",
        "beta = 100.0 # Strength of the constraint\n",
        "\n",
        "k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n",
        "k_factor.print(\"KarcherMeanFactorRot3: \")\n",
        "\n",
        "# Linearization example\n",
        "values = Values()\n",
        "values.insert(R(0), Rot3.Yaw(0.1))\n",
        "values.insert(R(1), Rot3.Yaw(0.2))\n",
        "values.insert(R(2), Rot3.Yaw(0.3))\n",
        "\n",
        "linearized_factor = k_factor.linearize(values)\n",
        "\n",
        "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n",
        "sqrt_beta = np.sqrt(beta)\n",
        "A = linearized_factor.getA()\n",
        "assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n",
        "A0 = A[:, :3]\n",
        "A1 = A[:, 3:6]\n",
        "A2 = A[:, 6:9]\n",
        "b = linearized_factor.getb()\n",
        "\n",
        "print(f\"sqrt(beta): {sqrt_beta}\")\n",
        "print(f\"\\nJacobian for R(0):\\n{A0}\")\n",
        "print(f\"Jacobian for R(1):\\n{A1}\")\n",
        "print(f\"Jacobian for R(2):\\n{A2}\")\n",
        "print(f\"Error vector b:\\n{b}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
